Class Estimator

Defined in File estimator.h

Class Documentation

class Estimator

Public Types

enum SolverFlag

Current optimization state.

estimator.h

Values:

enumerator INITIAL

enumerator NON_LINEAR

enum MarginalizationFlag

Marginalization types.

estimator.h

Values:

enumerator MARGIN_OLD: Marginalize out the oldest frame.

enumerator MARGIN_SECOND_NEW: Marginalize the last frame out of the sliding window and replace it with new key frame.

Public Functions

Estimator()

~Estimator()

void setParameter(): Set the covariance matrix of visual measurement reprojection error (ProjectionFactor)

void startProcessThread(): Start the main thread of processMeasurements().

void initFirstPose(const Eigen::Vector3d &p, const Eigen::Matrix3d r)

void inputIMU(double t, const Vector3d &linearAcceleration, const Vector3d &angularVelocity)

void inputINS(double t, const Vector3d &linearSpeed, const Quaterniond &angularRead, const double height)

void inputFeature(double t, const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &featureFrame)

void inputImage(double t, const cv::Mat &_img, const cv::Mat &_img1 = cv::Mat(), const cv::Mat &_mask = cv::Mat())

Interface to input image data into FeatureTracker class.

Parameters

t –
_img – Left image data.
_img1 – Right image data.
_mask – (Optional) image mask for filtering out dynamic objects (refers to left image).

void processIMU(double t, double dt, const Vector3d &linear_acceleration, const Vector3d &angular_velocity)

Process IMU data.

IMU Preintegration, use the mean value integration method to acquire current PQV as the initial value for optimization

Parameters

t – current time stamp
dt – time interval
linear_acceleration –
angular_velocity –

void processINS(double t, double dt, const Vector3d &linear_speed, const Quaterniond &angular_read, const double height_, const bool last_)

Process INSPVA data.

INSPVA data interpolation, according to the timestamp difference between INS sensor data and image data.

Parameters

t – Current time stamp
dt – Time interval between current and previous message time stamp.
linear_speed –
angular_read –
height_ – current height value
last_ – Flag of whether interpolation and slerp is needed. (last_ is true means the INSPVA message is not in the image gap middle, so there is time difference)

void processImage(const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, const double header)

Process image feature data.

Add feature points and calculates the tracking times and parallax to judge whether it is a key frame. Also does extrinsic calibration if online calibration is required. Then initialization is processed (VIO or VO) and sliding-window based optimization.

Parameters

image – Not the real “image” but the std::map containing all the feature properties and camera ID as index.
header –

void processMeasurements(): The main loop function of processing IMU, INS and image data into the estimator system.

void clearState(): Clear and initialize all the state values of the sliding window.

bool initialStructure()

visual structure initialization

to guarantee IMU is fully stimulated with motion.

Returns: bool true means initialization succeeds.

bool visualInitialAlign()

Joint initialization of VIO.

Note

Only used in IMU mode.

Returns: bool true means succeeds.

bool relativePose(Matrix3d &relative_R, Vector3d &relative_T, int &l)

Judge whether there is enough parallax (>30)

Parameters

relative_R – [out] Rotation matrix R between current frame and first frame
relative_T – [out] Translation vector T between current frame and first frame
l – [out] The corresponding frame that satisfies the initialization condition in sliding window

Returns

bool true can be initialized; false otherwise.

void slideWindow()

Process sliding window action.

If the second last frame is keyframe, then marginalizes the oldest frame and turn the feature points (and IMU observations) into prior knowledge. If not keyframe, abandons visual measurements (but keep the IMU observations to ensure continuity of preintegration)

void slideWindowNew(): Sliding window processes the observed frame count when marginalizing the second last frame and replacing it with new key frame.

void slideWindowOld(): Sliding window processes the observed frame count when marginalizing out the oldest frame to keep efficiency.

void optimization(): The main optimization including IMU factor, reprojection error factor and marginalization factor.

Note

This function SHOULD be called after the initial guess of PNP estimation (FeatureManager.initFramePoseByPnP and FeatureManager.triangulate).

void vector2double()

Copy the state vectors to parameter blocks for optimization process.

The state vectors include Ps(translation), Rs(rotation), Vs, Bas, Bgs, tic, ric(external matrix), feature depth vector.

void double2vector()

Copy the parameter blocks from optimization process to state vectors.

The parameter blocks include para_Pose(7), para_SpeedBias(9), para_Ex_Pose(7), Para_Feature(1), para_Td.

bool failureDetection()

Failure detection.

Returns: true if there is a failure detection.

bool getIMUInterval(double t0, double t1, vector<pair<double, Eigen::Vector3d>> &accVector, vector<pair<double, Eigen::Vector3d>> &gyrVector)

Put all the coming IMU messages into accVector and gyrVector between the interval of processMeasurements() sleep time.

Parameters

t0 – Last processing time stamp.
t1 – Current time stamp.
accVector – [out] Collection of linear acceleration data in small interval.
gyrVector – [out] Collection of angular velocity data in small interval.

Returns

False if no IMU messages received in the duration; Otherwise true.

bool getINSInterval(double t0, double t1, vector<pair<double, Eigen::Vector3d>> &spdVector, vector<pair<double, Eigen::Quaterniond>> &angVector, vector<pair<double, double>> &heightVector)

Put all the coming INS messages into spdVector and angVector between the interval of processMeasurements() sleep time.

Parameters

t0 – Last processing time stamp.
t1 – Current time stamp.
spdVector – [out] Collection of linear velocity data in small interval.
angVector – [out] Collection of angular data in small interval.

Returns

False if no INS messages received in the duration; Otherwise true.

void getPoseInWorldFrame(Eigen::Matrix4d &T)

Get the pose of current frame in world frame.

Parameters: T – [out] Inquired frame pose from current frame to world frame.

void getPoseInWorldFrame(int index, Eigen::Matrix4d &T)

Get the pose of inquired frame in world frame.

Parameters

index – Inquired frame.
T – [out] Inquired frame pose from inquired frame to world frame.

void predictPtsInNextFrame(): Predict next pose with assumption of constant velocity motion model.

void outliersRejection(set<int> &removeIndex)

Reject landmark feature outliers with reprojection error bounding.

Parameters: removeIndex – [out] std::set of indices to be removed.

double reprojectionError(Matrix3d &Ri, Vector3d &Pi, Matrix3d &rici, Vector3d &tici, Matrix3d &Rj, Vector3d &Pj, Matrix3d &ricj, Vector3d &ticj, double depth, Vector3d &uvi, Vector3d &uvj)

Calculate the 2D reprojection error given two frames i and j.

Parameters

Ri – Rotation matrix of ith body frame.
Pi – Translation vector of ith body frame.
rici – Extrinsic rotation matrix from camera frame to body frame, referring ith image.
tici – Extrinsic tranlation matrix from camera frame to body frame, referring ith image.
Rj – Rotation matrix of jth body frame.
Pj – Translation vector of jth body frame.
ricj – Extrinsic rotation matrix from camera frame to body frame, referring jth image.
ticj – Extrinsic tranlation matrix from camera frame to body frame, referring jth image.
depth – distance of the 3D point in the first observed frame.
uvi – homogeneous 2D point in ith normalized camera coordinate frame
uvj – homogeneous 2D point in jth normalized camera coordinate frame

Returns

The RMSE of 2D reprojection error on image coordinate.

void updateLatestStates()

void fastPredictIMU(double t, const Eigen::Vector3d &linear_acceleration, const Eigen::Vector3d &angular_velocity)

void fastPredictINS(double t, const Eigen::Vector3d &linear_speed, const Eigen::Quaterniond &angular_read)

bool IMUAvailable(double t)

bool INSAvailable(double t)

void initFirstIMUPose(vector<pair<double, Eigen::Vector3d>> &accVector)

void initFirstINSPose(vector<pair<double, Eigen::Vector3d>> &spdVector, vector<pair<double, Eigen::Quaterniond>> &angVector, vector<pair<double, double>> heightVector)

Public Members

unsigned int count_

std::mutex mBuf

std::mutex mProcess

queue<pair<double, Eigen::Vector3d>> accBuf

queue<pair<double, Eigen::Vector3d>> gyrBuf

queue<pair<double, Eigen::Vector3d>> spdBuf

queue<pair<double, Eigen::Quaterniond>> angBuf

queue<pair<double, double>> heightBuf

queue<pair<double, map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>>>> featureBuf

vector<pair<double, vector<double>>> gpsVec

double prevTime

double curTime

bool openExEstimation

std::thread processThread

atomic<bool> processThread_swt

FeatureTracker featureTracker

SolverFlag solver_flag

MarginalizationFlag marginalization_flag

Vector3d g

Matrix3d ric[2]

Vector3d tic[2]

Vector3d Ps[(WINDOW_SIZE + 1)]

Vector3d Vs[(WINDOW_SIZE + 1)]

Matrix3d Rs[(WINDOW_SIZE + 1)]

Vector3d Bas[(WINDOW_SIZE + 1)]

Vector3d Bgs[(WINDOW_SIZE + 1)]

double td

Matrix3d back_R0

Matrix3d last_R

Matrix3d last_R0

Vector3d back_P0

Vector3d last_P

Vector3d last_P0

double Headers[(WINDOW_SIZE + 1)]

double last_time

IntegrationBase *pre_integrations[(WINDOW_SIZE + 1)]

Vector3d acc_0

Vector3d gyr_0

Quaterniond ang_0

vector<double> dt_buf[(WINDOW_SIZE + 1)]

vector<double> t_buf[(WINDOW_SIZE + 1)]

vector<Vector3d> linear_acceleration_buf[(WINDOW_SIZE + 1)]

vector<Vector3d> angular_velocity_buf[(WINDOW_SIZE + 1)]

vector<Vector3d> linear_speed_buf[(WINDOW_SIZE + 1)]

vector<Quaterniond> angular_read_buf[(WINDOW_SIZE + 1)]

double sum_dt[(WINDOW_SIZE + 1)]

int frame_count

int sum_of_outlier

int sum_of_back

int sum_of_front

int sum_of_invalid

int inputImageCnt

FeatureManager f_manager

MotionEstimator m_estimator

InitialEXRotation initial_ex_rotation

bool first_imu

bool first_ins

bool is_valid

bool is_key

bool failure_occur

vector<Vector3d> point_cloud

vector<Vector3d> margin_cloud

vector<Vector3d> key_poses

double initial_timestamp

double para_Pose[WINDOW_SIZE + 1][SIZE_POSE]

double para_SpeedBias[WINDOW_SIZE + 1][SIZE_SPEEDBIAS]

double para_Feature[NUM_OF_F][SIZE_FEATURE]

double para_Ex_Pose[2][SIZE_POSE]

double para_Retrive_Pose[SIZE_POSE]

double para_Td[1][1]

double para_Tr[1][1]

double sensor_h

int loop_window_index

MarginalizationInfo *last_marginalization_info

vector<double*> last_marginalization_parameter_blocks

map<double, ImageFrame> all_image_frame

IntegrationBase *tmp_pre_integration

Eigen::Matrix3d cov_position

Eigen::Vector3d initP

Eigen::Matrix3d initR

double latest_time

Eigen::Vector3d latest_P

Eigen::Vector3d latest_V

Eigen::Vector3d latest_Ba

Eigen::Vector3d latest_Bg

Eigen::Vector3d latest_acc_0

Eigen::Vector3d latest_gyr_0

Eigen::Vector3d last_vec_rev

Eigen::Vector3d latest_spd_0

Eigen::Quaterniond latest_Q

Eigen::Quaterniond last_ang_rev

bool initFirstPoseFlag

bool initThreadFlag